package FRC2856.Robot;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Solenoid;

public class UserInterface{
    
    /*this class takes the two joysticks and uses them to contorl the robot.
     * this class mostly deals with button mapping.
     * 
     * 
     */

    //DriverStation station;
    Joystick driveStick, shooterStick;//the joysticks for the driver and shooter
    Joystick dtStick;//joystick for testing the drivetrain
    Joystick shtrStick;//joystick for testing the shooter
    DriveTrain dt;//the drive train
    Shooter cannon; //the shooter
    Capture cap; //the ball control mechanism
    Aiming aim; //the camera and aim system
    BridgeArm arm;
    Relay light;
    boolean lightOn = false;
    CustomButton[] shooterButtons = new CustomButton[12];
    CustomButton[] driverButtons = new CustomButton[12];
    CustomButton[] dtButtons = new CustomButton[12];
    CustomButton[] shtrButtons = new CustomButton[12];
    double dtZ;
    boolean dtRev;
    double shtrZ;
    private boolean pidShooterControl = false;
    Solenoid sol;
    
    public UserInterface(Shooter s, DriveTrain d, Aiming a, Capture c, BridgeArm ba){
        driveStick = new Joystick(1);
        shooterStick = new Joystick(2);
        dtStick = new Joystick(3);
        shtrStick = new Joystick(4);
        arm= ba;
        cannon= s;
        dt= d;
        aim= a;
        cap= c;
        setupButtons();
        //station=new DriverStation();
        light = aim.pro.cam.LED;
        dtRev = false;
        sol = new Solenoid(1,1);
    }
    
    /*If it turns out we do want to use Xbox 360 or ps3 controllers, the button mapping should be similar.
     *  There is a separate Xbox360 controller class that extends I made if we need that
     * 
     * To my understanding we are most likely going to use joysticks
     * What's going on in here?
     */
    
    public void update(){//call this continuously in the robot code
        //shooter stick stuff
        periodicButtons();
        
        
        
        if(shooterButtons[1].status){//press the trigger on the shooter joystick
            if(shooterButtons[3].status)
                cap.setUpperSpeed(-1);
            else
                cap.setUpperSpeed(1);
            //this.fire();//this class's fire function calls the aimer, aquires a target, then fires
        }
        if(shooterButtons[1].getChangedStatus(false))
            cap.setUpperSpeed(0);

        if(shooterButtons[2].getChangedStatus(true)){
            //aim bottom basket
            //aim.aim(3);
        }
        
        if(shooterButtons[3].getChangedStatus(true)){
            //aim top basket
            //aim.aim(0);
        }
        
        double shootYAxis = shooterStick.getY();
        if(shooterButtons[4].status || shooterButtons[5].status){
            if(shootYAxis>0.05)
                arm.setMotion(shootYAxis/1);
            else if(shootYAxis<-0.05)
                arm.setMotion(shootYAxis/4);
            else
                arm.setMotion(0);
        }
        else{
            arm.setMotion(0);
            if(shooterStick.getY()>=0.1 || shooterStick.getY()<=-0.1)
                aim.pro.cam.rotateServo(shooterStick.getY()/100);   
        }
        
        if(shooterButtons[6].getChangedStatus(true)){
            cap.setLowerSpeed(cap.getLowerSpeed()+1);
        }
        if(shooterButtons[7].getChangedStatus(true)){
            cap.setLowerSpeed(cap.getLowerSpeed()-1);
        }
        
        if(shooterButtons[8].getChangedStatus(true)){
            pidShooterControl = true;
        }
        if(shooterButtons[9].getChangedStatus(true)){
            cannon.spinStop();
            pidShooterControl = false;
        }
        
        if(pidShooterControl){
            cannon.spinRateUpdate((RobotMap.shtrSpeedMax-RobotMap.shtrSpeedMin) * (1-shooterStick.getZ())/2 + RobotMap.shtrSpeedMin);
        }
        else{
            cannon.spinEffort(RobotMap.shtrEffortMax*(1-shooterStick.getZ())/2);//z axis adjusts speed of the shooter's wheels
        }
        
        if(shooterButtons[10].getChangedStatus(true)){
            //System.out.println("C");
            lightOn = !lightOn;
            if(lightOn)
                light.set(Relay.Value.kOn);
            else
                light.set(Relay.Value.kOff);
        }
        
        /*
        if(shooterButtons[9].getChangedStatus(false))
            arm.setMotion(0);
        
        if(shooterButtons[10].getChangedStatus(true)){
            cap.setUpperSpeed(cap.getUpperSpeed()-1);
        }
        if(shooterButtons[11].getChangedStatus(true)){
            cap.setUpperSpeed(cap.getUpperSpeed()+1);
        }
        */
        //drive stick stuff
        dt.arcadeDrive(driveStick);
        
        if(driverButtons[1].getChangedStatus(true)){//override
            maxPower(true);
        }
        else if(driverButtons[1].getChangedStatus(false)){
            maxPower(false);
        }

        if(driverButtons[2].getChangedStatus(true)){
            sol.set(true);
        }
        else if(driverButtons[2].getChangedStatus(false)){
            sol.set(false);
        }
        
        /*byte armAction = 0; //binary encoding (1,2,4,8...)
        if(driverButtons[2].status){
            armAction |= 1;
        }
        if(driverButtons[3].status){
            armAction |= 2;
        }

        if(driverButtons[6].status){
            armAction |= 4;
        }
        if(driverButtons[7].status){
            armAction |= 8;
        }
          
         
        switch(armAction){
            case 1:
                //lower the arm quickly
                arm.setMotion(0.1);
               break;
            case 2:
                //raise the arm quickly
                arm.setMotion(-0.1);
                break;
            case 4:
                //lower the arm slowely
                arm.setMotion(0.05);
                break;
            case 8:
                //raise the arm slowely
                arm.setMotion(-0.05);
                break;
            default:
                //no buttons or more than one button pressed - turn off
                arm.setMotion(0.0);
        }*/
        //System.out.println(arm.mover.get());
        
        
        if(driverButtons[4].getChangedStatus(true)){
            double[] encoderDist = dt.getEncoderDist();
            System.out.println("Dist(L:R) " + encoderDist[0] + ":" + encoderDist[1]);
        }

        if(driverButtons[5].getChangedStatus(true)){
            double[] encoderRate = dt.getEncoderRate();
            System.out.println("Rate(L:R) " + encoderRate[0] + ":" + encoderRate[1]);
        }

        if(driverButtons[8].getChangedStatus(true)){
            double[] encoderDist = cannon.getEncoderDist();
            System.out.println("Dist(L:R) " + encoderDist[0] + ":" + encoderDist[1]);
        }

        if(driverButtons[9].getChangedStatus(true)){
            double[] encoderRate = cannon.getEncoderRate();
            System.out.println("Rate(L:R) " + encoderRate[0] + ":" + encoderRate[1]);
        }

        /*
        //if(driverButtons[8].status && driverButtons[9].status)
        //    dt.autoBalance();
 
        if(driverButtons[8].getChangedStatus(true)){
            System.out.println("Angle: " + dt.getGyro());
        }
        
        if(driverButtons[9].getChangedStatus(true)){
            double[] accels = dt.getAccelerations();
            System.out.println("Accel-X: " + accels[0] + " Y: " + accels[1] + " Z: " + accels[2]);
        }
        
        if(driverButtons[10].status){
            cap.numBalls=0;
        }
        */

        // Drivertrain testing stuff
        //dt.arcadeDrive(dtStick);

//        if(dtStick.getZ() != dtZ){
//            dtZ = dtStick.getZ();
//            double dtZx = (1.0 - dtZ)/2.0;
//            dt.setOutputRange(dtZx);
//            System.out.println("PID Range: " + dtZx);
//        }
        /*
        if(dtButtons[1].getChangedStatus(true)){
            dtRev = !dtRev;
            if(dtRev){
                System.out.println("dt Rev");
            }
            else{
                System.out.println("dt Fwd");
            }
        }

        if(dtButtons[2].getChangedStatus(true)){
            dt.moveStop();
            System.out.println("moveStop");
        }

        if(dtButtons[3].getChangedStatus(true)){
            if(dtRev){
                dt.moveDist(-2.0, -2.0);
            }
            else{
                dt.moveDist(2.0, 2.0);
            }
            System.out.println("moveDist");
        }

        if(dtButtons[4].getChangedStatus(true)){
            double[] encoderDist = dt.getEncoderDist();
            System.out.println("Dist(L:R) " + encoderDist[0] + ":" + encoderDist[1]);
        }

        if(dtButtons[5].getChangedStatus(true)){
            double[] encoderRate = dt.getEncoderRate();
            System.out.println("Rate(L:R) " + encoderRate[0] + ":" + encoderRate[1]);
        }

        if(dtButtons[6].getChangedStatus(true)){
            double[] PID = dt.incrementP();
            System.out.println("New P:" + PID[0] + ":" + PID[1]);
        }

        if(dtButtons[7].getChangedStatus(true)){
            double[] PID = dt.decrementP();
            System.out.println("New P:" + PID[0] + ":" + PID[1]);
        }

        if(dtButtons[8].getChangedStatus(true)){
            double[] PID = dt.decrementI();
            System.out.println("New I:" + PID[0] + ":" + PID[1]);
        }

        if(dtButtons[9].getChangedStatus(true)){
            double[] PID = dt.incrementI();
            System.out.println("New I:" + PID[0] + ":" + PID[1]);
        }

        if(dtButtons[10].getChangedStatus(true)){
            double[] PID = dt.decrementD();
            System.out.println("New D:" + PID[0] + ":" + PID[1]);
        }

        if(dtButtons[11].getChangedStatus(true)){
            double[] PID = dt.incrementD();
            System.out.println("New D:" + PID[0] + ":" + PID[1]);
        }
        */

        // Shooter testing stuff

//        if(shtrStick.getZ() != shtrZ){
//            shtrZ = dtStick.getZ();
//            double shtrZx = (1.0 - shtrZ)/2.0;
//            cannon.setOutputRange(shtrZx);
//            System.out.println("PID Range: " + shtrZx);
//        }

//        if(shtrButtons[1].getChangedStatus(true)){
//            cap.setUpperSpeed(1.0);
//        }
//        else if (shtrButtons[1].getChangedStatus(false)){
//            cap.setUpperSpeed(0.0);
//        }
        /*
        if(shtrButtons[2].getChangedStatus(true)){
            cannon.spinStop();
            System.out.println("spinStop");
        }

        if(shtrButtons[3].getChangedStatus(true)){
            cannon.spinRateUpdate(Math.abs(shtrStick.getY())*100.0);
            System.out.println("spinRate");
        }

        if(shtrButtons[4].getChangedStatus(true)){
            double[] encoderDist = cannon.getEncoderDist();
            System.out.println("Dist(L:R) " + encoderDist[0] + ":" + encoderDist[1]);
        }

        if(shtrButtons[5].getChangedStatus(true)){
            double[] encoderRate = cannon.getEncoderRate();
            System.out.println("Rate(L:R) " + encoderRate[0] + ":" + encoderRate[1]);
        }

        if(shtrButtons[6].getChangedStatus(true)){
            double[] PID = cannon.incrementP();
            System.out.println("New P:" + PID[0] + ":" + PID[1]);
        }

        if(shtrButtons[7].getChangedStatus(true)){
            double[] PID = cannon.decrementP();
            System.out.println("New P:" + PID[0] + ":" + PID[1]);
        }

        if(shtrButtons[8].getChangedStatus(true)){
            double[] PID = cannon.decrementI();
            System.out.println("New I:" + PID[0] + ":" + PID[1]);
        }

        if(shtrButtons[9].getChangedStatus(true)){
            double[] PID = cannon.incrementI();
            System.out.println("New I:" + PID[0] + ":" + PID[1]);
        }

        if(shtrButtons[10].getChangedStatus(true)){
            double[] PID = cannon.decrementD();
            System.out.println("New D:" + PID[0] + ":" + PID[1]);
        }

        if(shtrButtons[11].getChangedStatus(true)){
            double[] PID = cannon.incrementD();
            System.out.println("New D:" + PID[0] + ":" + PID[1]);
        }
        */
    }
    
    /*This function will cause all power to go to the drive train 
     * and cut off all other functionality until it is disabled...i think
     */
    public void maxPower(boolean enable){
       cap.maxPower(enable);       
    }

    /*
     * this function should tell the targeting system to aquire the target, aim the cannon, and FIRE
     * 
     */
    private void fire() {
        //aim.aim(0);
        //cannon.operatorControl();
        
    }

    

    private void setupButtons() {
        System.out.println("Setting Up Buttons");
        for(int i = 1; i<12; i++){
            shooterButtons[i] = new CustomButton(shooterStick, i);
            driverButtons[i] = new CustomButton(driveStick, i);
            dtButtons[i] = new CustomButton(dtStick, i);
            shtrButtons[i] = new CustomButton(shtrStick, i);
        }
    }
    
    private void periodicButtons() {
        for(int i = 1; i<12; i++){
            shooterButtons[i].periodicChanged();
            driverButtons[i].periodicChanged();
            dtButtons[i].periodicChanged();
            shtrButtons[i].periodicChanged();
        }
    }
    
    private class CustomButton{
        Joystick joy;
        int index;
        boolean changed = false;
        boolean status = false;
        public CustomButton(Joystick j, int i){
            joy = j;
            index = i;
            status = joy.getRawButton(index);
        }
        
        public boolean getRawStatus(){
            status = joy.getRawButton(index);
            return joy.getRawButton(index);
        }
        
        public boolean getChangedStatus(boolean compareTo){
            return (changed) && (status == compareTo);
        }
        
        public void periodicChanged(){
            changed = (status != joy.getRawButton(index));
            status = joy.getRawButton(index);
        }
    }  
}
